#include "native_debug.h"
#include "stereo_depth.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <sstream>
#include <algorithm>

// phone series
#define P_VENICE 0
#define P_OAKLAND 1
#define P_AK57 2
#define P_HONGKONG 3
#define P_HONGKONG_PRO 4
#define P_VGA 5
#define P_T1PRO 6
#define P_OTTAWA 7

#define PRODUCT_ID P_T1PRO

#if PRODUCT_ID>=P_T1PRO
#define CALIB_VERSION 8
#else
#define CALIB_VERSION 7
#endif

#if CALIB_VERSION == 7
	#define DIST_PARAM_NUM 5
#else
	#define DIST_PARAM_NUM 8
#endif


struct CalibrationFile {
#if CALIB_VERSION == 8
	int phone_id;
#endif
	int full_size_w;
	int full_size_h;
	int crop_size_w;
	int crop_size_h;
	int orientation;
	int size_w;
	int size_h;
	float MIN_DISP;
	float MAX_DISP;
	float delta_disp_per_degree;
	double cam_mat_sub[3][3]; // camera matrix of sub 
	double dist_coef_sub[1][DIST_PARAM_NUM]; // distortion coefficient of sub
	double cam_mat_main[3][3]; // camera matrix of main
	double dist_coef_main[1][DIST_PARAM_NUM]; // distortion coefficient of main
	double rot[3][3]; // rotation between two cameras
	double trans[3][1]; // translation between two cameras
};


bool GetSortedImagePathPairs(
        std::vector<cv::String> & files_aux,
        std::vector<cv::String> & files_main,
        const cv::String & input_dir
){
    std::vector<cv::String> jpg_aux, files_aux_unsorted;
    std::vector<cv::String> jpg_main, files_main_unsorted;

    cv::glob(input_dir + "/picAux/*.jpg", jpg_aux);
	cv::glob(input_dir + "/picAux/*.png", files_aux_unsorted);
	files_aux_unsorted.insert(files_aux_unsorted.end(), jpg_aux.begin(), jpg_aux.end());
	
	cv::glob(input_dir + "/picMain/*.jpg", jpg_main);
	cv::glob(input_dir + "/picMain/*.png", files_main_unsorted);
	files_main_unsorted.insert(files_main_unsorted.end(), jpg_main.begin(), jpg_main.end());

    if(files_aux_unsorted.size() != files_main_unsorted.size()){
        return false;
    }

    std::vector<std::pair<int, int>> pair_aux;
    std::vector<std::pair<int, int>> pair_main;

    std::stringstream ss("");
    int idx;

    for (int i = 0; i < files_aux_unsorted.size(); ++i) {
        ss.clear();
        ss.str("");
		int pos_beg = (input_dir + "/picAux/").size();
		int seg_id = files_aux_unsorted[i].find('_', pos_beg);
        ss << files_aux_unsorted[i].substr(pos_beg, seg_id - pos_beg);
        ss >> idx;
        pair_aux.push_back(std::pair<int, int>(idx, i));
        ss.clear();
        ss.str("");
		pos_beg = (input_dir + "/picMain/").size();
		seg_id = files_main_unsorted[i].find('_', pos_beg);
		ss << files_main_unsorted[i].substr(pos_beg, seg_id - pos_beg);
        ss >> idx;
        pair_main.push_back(std::pair<int, int>(idx, i));
    }

    std::sort(pair_aux.begin(), pair_aux.end());
    std::sort(pair_main.begin(), pair_main.end());

    for (int i = 0; i < pair_aux.size(); ++i) {
        files_aux.push_back(files_aux_unsorted[pair_aux[i].second]);
        files_main.push_back(files_main_unsorted[pair_main[i].second]);
    }
    return true;
}


int run(const cv::String & base_dir, const cv::String date_id, const cv::String phone_id) {
	double beg_, end_;
	double freq = cv::getTickFrequency();
	r3d_depth_image::R3DStereoDepth *m_DepthAlgorithmPtr = new r3d_depth_image::R3DStereoDepth;
	r3d_depth_image::ParamStereo param_depth;

	cv::String ori_dir = "ori/";
	cv::String cal_dir = "cal/";
	cv::String depth_dir = "depth/";
	cv::String dump_dir = "dump/";
#if CALIB_VERSION == 7
	cv::String cal_ver = "version7";
#else
	cv::String cal_ver = "version8";
#endif

	// calibration settings
	int orientation;
	cv::Size cali_size;
	cv::Size full_size;
	cv::Size crop_size;
	float delta_disp_per_degree;
	cv::Mat cam_mat_sub, dist_coef_sub, cam_mat_main, dist_coef_main, rot, trans;
	float min_disp;

	String cali_bin = base_dir + "/" + cal_dir + "/" + phone_id + "/" + cal_ver + "/dualcam_cali.bin";
	//String cali_bin = base_dir + "/" + cal_dir + "/" + phone_id + "/" + cal_ver + "/R3DGoldenFile.bin";
	std::fstream ifs(cali_bin, std::ios::in | std::ios::binary);
	CalibrationFile califile_in;
	if(!ifs.is_open()){
		IMAGE_DEPTH_LOGE("dualcam_cali.bin not found!");
		return - 1;
	}
	ifs.read((char*)&califile_in, sizeof(califile_in));
	
	orientation = califile_in.orientation;
	cali_size.width = califile_in.size_w;
	cali_size.height = califile_in.size_h;
	full_size.width = califile_in.full_size_w;
	full_size.height = califile_in.full_size_h;
	crop_size.width = califile_in.crop_size_w;
	crop_size.height = califile_in.crop_size_h;
	delta_disp_per_degree = califile_in.delta_disp_per_degree;
	cam_mat_sub = cv::Mat(Size(3, 3), CV_64FC1, califile_in.cam_mat_sub);
	dist_coef_sub = cv::Mat(Size(1, DIST_PARAM_NUM), CV_64FC1, califile_in.dist_coef_sub);
	cam_mat_main = cv::Mat(Size(3, 3), CV_64FC1, califile_in.cam_mat_main);
	dist_coef_main = cv::Mat(Size(1, DIST_PARAM_NUM), CV_64FC1, califile_in.dist_coef_main);
	rot = cv::Mat(Size(3, 3), CV_64FC1, califile_in.rot);
	trans = cv::Mat(Size(3, 1), CV_64FC1, califile_in.trans);
	min_disp = califile_in.MIN_DISP;

//#define CUSTOM_CROP
#ifdef CUSTOM_CROP
	full_size.width = 3264;
	full_size.height = 2448;
	crop_size.width = 2176;
	crop_size.height = 1632;
#endif


	///////////////// Calibration Data Settings //////////////////
	param_depth.orientation = orientation;
	param_depth.cali_size = cali_size;
	param_depth.full_size = full_size;
	param_depth.crop_size = crop_size;
	param_depth.delta_disp_per_degree = delta_disp_per_degree;
	param_depth.intrinsics.cam_mat_sub = cam_mat_sub;
	param_depth.intrinsics.dist_coef_sub = dist_coef_sub;
	param_depth.intrinsics.cam_mat_main = cam_mat_main;
	param_depth.intrinsics.dist_coef_main = dist_coef_main;
	param_depth.extrinsics.rot = rot;
	param_depth.extrinsics.trans = trans;
	
	param_depth.min_disp = min_disp;

	std::cout << "full_size = " << param_depth.full_size << std::endl;
	std::cout << "crop_size = " << param_depth.crop_size << std::endl;
	std::cout << "cali_size = " << param_depth.cali_size << std::endl;
	std::cout << "cam_mat_sub = " << cam_mat_sub << std::endl;
	std::cout << "dist_coef_sub = " << dist_coef_sub << std::endl;
	std::cout << "cam_mat_main = " << cam_mat_main << std::endl;
	std::cout << "dist_coef_main = " << dist_coef_main << std::endl;
	std::cout << "rot = " << rot << std::endl;
	std::cout << "trans = " << trans << std::endl;
	std::cout << "min_disp = " << min_disp << std::endl;
	
	////////// Constants Depending on Phone Series ///////////////
#if PRODUCT_ID==P_VENICE
	// venice settings
	param_depth.delta_disp_per_degree = 12.5;
	param_depth.full_size = cv::Size(4608, 3456);
	param_depth.crop_size = cv::Size(3264, 2448);
	param_depth.orientation = R3D_IMAGE_ROTATE_NONE;
#elif PRODUCT_ID==P_OAKLAND
	// oakland settings
	param_depth.delta_disp_per_degree = 12.5;
	param_depth.full_size = cv::Size(4608, 3456);
	param_depth.crop_size = cv::Size(4377, 3283);
	param_depth.orientation = R3D_IMAGE_ROTATE_COUNTER_CLOCKWISE_90;
#elif PRODUCT_ID==P_HONGKONG
	param_depth.intrinsics.D1.at<double>(0, 0) *= 0.6;
	param_depth.intrinsics.D1.at<double>(0, 1) *= 0.6;
	param_depth.intrinsics.D1.at<double>(0, 4) *= 0.6;
#elif PRODUCT_ID==P_HONGKONG_PRO
	param_depth.intrinsics.M1.at<double>(0, 2) = (1296.0 + M1.at<double>(0, 2)) / 2.0;
	param_depth.intrinsics.M1.at<double>(1, 2) = (972.0 + M1.at<double>(1, 2)) / 2.0;
	param_depth.intrinsics.M2.at<double>(0, 2) = (1296.0 + M2.at<double>(0, 2)) / 2.0;
	param_depth.intrinsics.M2.at<double>(1, 2) = (972.0 + M2.at<double>(1, 2)) / 2.0;
	double P1, P2;
	P1 = D1.at<double>(0, 2);
	P2 = D1.at<double>(1, 2);
	param_depth.intrinsics.D1 = cv::Mat::zeros(cv::Size(8, 1), CV_64FC1);
	param_depth.intrinsics.D1.at<double>(0, 0) = 15.272738270574715;
	param_depth.intrinsics.D1.at<double>(0, 1) = -15.58850643109654;
	param_depth.intrinsics.D1.at<double>(0, 2) = P1;
	param_depth.intrinsics.D1.at<double>(0, 3) = P2;
	param_depth.intrinsics.D1.at<double>(0, 4) = 15.742645957633842;
	param_depth.intrinsics.D1.at<double>(0, 5) = 15.401803699170106;
	param_depth.intrinsics.D1.at<double>(0, 6) = -16.751108150356252;
	param_depth.intrinsics.D1.at<double>(0, 7) = 16.678848641257932;

	P1 = D2.at<double>(0, 2);
	P2 = D2.at<double>(1, 2);
	param_depth.intrinsics.D2 = cv::Mat::zeros(cv::Size(8, 1), CV_64FC1);
	param_depth.intrinsics.D2.at<double>(0, 0) = 1.823495010104061;
	param_depth.intrinsics.D2.at<double>(0, 1) = 44.68283278863989;
	param_depth.intrinsics.D2.at<double>(0, 2) = P1;
	param_depth.intrinsics.D2.at<double>(0, 3) = P2;
	param_depth.intrinsics.D2.at<double>(0, 4) = -45.015240843762165;
	param_depth.intrinsics.D2.at<double>(0, 5) = 1.8121933086810782;
	param_depth.intrinsics.D2.at<double>(0, 6) = 43.76376001239106;
	param_depth.intrinsics.D2.at<double>(0, 7) = -44.11918951361282;
#endif

	////////////// Depth Algorithm Settings //////////////////////
	param_depth.num_thread = 4;
#if PRODUCT_ID == P_T1PRO || PRODUCT_ID == P_OTTAWA
	param_depth.alg_type = R3D_IMAGE_DEPTH_ALG_BEST;
#else
	param_depth.alg_type = R3D_IMAGE_DEPTH_ALG_NORMAL;
#endif
	/////////////////////// Debug Settings ///////////////////////
	param_depth.dump_enabled = false;
	param_depth.dump_path = base_dir + dump_dir + date_id + "/" + phone_id + "/";
	////////////////////////// SETUP DONE ////////////////////////

	cv::String input_dir = base_dir + ori_dir + date_id + "/" + phone_id + "/";
	cv::String output_dir = base_dir + depth_dir + date_id + "/" + phone_id + "/";
	std::vector<cv::String> files_aux, files_main;
	GetSortedImagePathPairs(files_aux, files_main, input_dir);

	// create a global uid for synchronize log/dump information
	// Example. 11_CP-Bokeh-imgDest_1580053864_3264x2448_doflevel32_focus1172_1321
	// or Example. 3_CP-Bokeh-Main1_2059989081_3264x2448_doflevel32_focus1590_1187.nv21_Main
	cv::String uid;
	int fx, fy;

	for (int i = 8; i<files_aux.size(); ++i) {
		cv::Mat im_aux = cv::imread(files_aux[i], cv::IMREAD_GRAYSCALE);
		cv::Mat im_main = cv::imread(files_main[i], cv::IMREAD_COLOR);
		
		beg_ = cv::getTickCount();
		cv::Mat depth;
		int fn_idx = files_main[i].find_last_of('\\');
		uid = files_main[i].substr(fn_idx + 1, files_main[i].size() - 5 - fn_idx);
		IMAGE_DEPTH_LOGI("uid: %s", uid.c_str());
		// get the focus coordinate
		//GetFocus(uid, fx, fy);
		//IMAGE_DEPTH_LOGI("focus: (%d, %d)", fx, fy);

//#define SIMULATION
#ifdef SIMULATION
		cv::resize(im_main, im_main, param_depth.full_size);
		im_main = im_main(
			Rect(
				(param_depth.full_size.width - param_depth.crop_size.width) / 2, 
				(param_depth.full_size.height - param_depth.crop_size.height) / 2,
				param_depth.crop_size.width, 
				param_depth.crop_size.height)
			);
		r3d_depth_image::DumpImage(im_main, output_dir, "", uid);
#endif

#ifdef CUSTOM_CROP
		cv::Mat main_small;
		cv::resize(im_main, main_small, cv::Size(816, 612));
		cv::imshow("origin", main_small);
		// from 3264 to 2448, 2560 x 1920
		im_main = im_main(Rect(
			(full_size.width-crop_size.width)/2, 
			(full_size.height-crop_size.height)/2, 
			crop_size.width, crop_size.height));
		cv::resize(im_main, im_main, cv::Size(3264, 2448));
#endif


//#define FRAME_SYNC
#ifdef FRAME_SYNC
		cv::resize(im_main, im_main, param_depth.crop_size);
		// change the focus in UID
		uid = uid.substr(0, uid.length() - 7);
		uid = uid + cv::String("1632_1224");
		r3d_depth_image::DumpImage(im_main, output_dir, "", uid);
#endif

		m_DepthAlgorithmPtr->Init(param_depth);
		//m_DepthAlgorithmPtr->Compute(depth, im_aux, im_main, uid,
		//	1.0*fx / param_depth.crop_size.width, 1.0*fy / param_depth.crop_size.height);
		m_DepthAlgorithmPtr->Compute(depth, im_aux, im_main, uid, 0, 0);
		//cv::destroyAllWindows();
		end_ = cv::getTickCount();
		IMAGE_DEPTH_LOGI("Depth Computation Cost: %8.3f ms.", (1000 * (end_ - beg_) / freq));
		r3d_depth_image::DumpImage(depth, output_dir, "depth_tcl", uid);
	}
	delete(m_DepthAlgorithmPtr);
	return 0;
}



int main(int argc, const char ** argv){
	if (argc != 4) {
		IMAGE_DEPTH_LOGE("Invalid Arguments!");
		IMAGE_DEPTH_LOGI("Format: bottle.exe [base_dir] [date_id] [phone_id]");
#if PRODUCT_ID==P_OAKLAND
		run("F:/dump/TEST-ALL/Dataset/image/", "20200901", "oakland0068");
#elif PRODUCT_ID==P_VENICE
		run("F:/dump/TEST-ALL/Dataset/image/", "20190929", "Venice02");
#elif PRODUCT_ID==P_AK57
		run("F:/dump/TEST-ALL/Dataset/image/", "20200901", "ak57-0071");
#elif PRODUCT_ID==P_HONGKONG
		run("F:/dump/TEST-ALL/Dataset/image/", "20210201", "HongKong0074");
		//run("F:/dump/TEST-ALL/Dataset/image/", "20201116", "HongKong0075");
#elif PRODUCT_ID==P_HONGKONG_PRO
		run("F:/dump/TEST-ALL/Dataset/image/", "20201229", "HongKongPro0078");
#elif PRODUCT_ID==P_VGA
		run("F:/dump/TEST-ALL/Dataset/image/", "20210125", "vga");
#elif PRODUCT_ID==P_T1PRO
		run("F:/dump/TEST-ALL/Dataset/image/", "20210203", "T1Pro0116");
#elif PRODUCT_ID==P_OTTAWA
		run("F:/dump/TEST-ALL/Dataset/image/", "20210420", "Ottawa0118");
#else
		IMAGE_DEPTH_LOGE("PLZ CHOOSE A PRODUCT TO RUN DEMO!");
#endif
		//system("pause");
		return 0;
	}
	else {
		IMAGE_DEPTH_LOGI("base_dir=%s, date_id=%s, phone_id=%s", argv[1], argv[2], argv[3]);
		run(argv[1], argv[2], argv[3]);
	}
	return 0;
}
